/*
 * navigation.cpp
 *
 * for static/dynamic motion planning
 *
 * useful commands: rosparam list
 * useful links: http://wiki.ros.org/roscpp/Overview/Parameter%20Server
 *               http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning
 */

#include <ros/ros.h>
#include <ros/callback_queue.h>
 
#include "nxt_lab2/UpdatedMap.h"
#include "nxt_lab2/MoveToCell.h"
#include "nxt_lab2/Neighbors.h"

#include <stdlib.h> // srand rand
#include <iostream>

/*********************************** GLOBAL VARIABLES ***********************************/

int empty_value = 0;
int start_value = -1;
int goal_value = 1;
int obstacle_value = -2;

int north_dir = 0; 
int east_dir = 1;
int south_dir = 2;
int west_dir = 3;

std::vector<int> world_map_vec;
int rows = 0;
int cols = 0;

int nxt_pos = -1;
int start_pos = -1;
int goal_pos = -1;

//bool dynamic_map = false;

/*************************************** SERVICES ***************************************/


/*
	This function provides the service for sending the updated map, 
	it takes in the request and response type defined in the srv file and returns a boolean.
*/
bool send_updated_map(nxt_lab2::UpdatedMap::Request  & req,
                      nxt_lab2::UpdatedMap::Response & res)
{
  res.rows = rows;
  res.cols = cols;
  res.updated_map = world_map_vec;
  res.nxt_pos = nxt_pos;

  ROS_INFO("Serving request for send_updated_map");

  return true;
}

/********************************** PRINTING FUNCTIONS *********************************/

/*
    Print vector of int

    Params:
      std::vector<int> & v: vector to be printed
*/
void printVec(std::vector<int> & v)
{
  for(int i = 0; i < v.size(); i++)
    std::cout << v[i] << "\t";
  std::cout << std::endl;
}

/*
    Print matrix represented as vector

    Params:
      std::vector<int> & map: vector representing matrix
*/
void printMap(std::vector<int> & map)
{
  for (int i = 0; i < rows; i++)
  { 
    for (int j = 0; j < cols; j++)
      std::cout << map[i * cols + j] << "\t";
    std::cout << std::endl;
  }
}

/********************************* NAVIGATION FUNCTIONS ********************************/

/*
    Find start index (from 0 to rows * cols - 1)
    return true if found, false otherwise

    Params:
      std::vector<int> & v: vector representing map
      int & start_index: index of found start
*/
bool find_start(std::vector<int> & v, int & start_index)
{
  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] == start_value)
    {
      start_index = i;
      return true;
    } 
  }
  return false;
} 

/*
    Find goal index (from 0 to rows * cols - 1)
    return true if found, false otherwise

    Params:
      std::vector<int> & v: vector representing map
      int & goal_index: index of found goal
*/
bool find_goal(std::vector<int> & v, int & goal_index)
{
  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] == goal_value)
    {
      goal_index = i;
      return true;
    } 
  }
  return false;
}


/*
    Return row index (from 0 to rows - 1) of vector position pos

    Params:
      int pos: vector position pos
*/
int get_row_index(int pos)
{
  return pos / cols; // int / int = int
}

/* 
    Find indexes of neighbors of cell with index pos (first element of pairs)
    storing its parent (second element of each pair), useful when it is called by fillMap
    and filtering obstacles
    
    Params:
      int pos: index of cell
      std::vector<int> & map: vector representing map
      std::list< std::pair <int,int> > & neighbors: indexes of neighbors of cell
      bool only_empty: if true, only neighbors with empty value and start value are returned,
                       option exploited by fillMap
*/
bool get_neighbors(int pos, std::vector<int> & map, std::list< std::pair <int,int> > & neighbors, bool only_empty = false)
{

  int row_index = get_row_index(pos);

  int nesw_cell_index[4] = {
    pos - cols , 
    pos + 1, 
    pos + cols, 
    pos - 1
  };

  bool valid_cell[4] = {
    pos - cols >= 0,
    pos + 1 < (row_index + 1) * cols,
    pos + cols < rows * cols,
    pos - 1 > row_index * cols - 1
  };

  for(int i = 0; i<4; i++)
    if (valid_cell[i] && 
       ((!only_empty && map[nesw_cell_index[i]] != obstacle_value) || // ho aggiunto !only_empty
       (only_empty && (map[nesw_cell_index[i]] == empty_value || map[nesw_cell_index[i]] == start_value)))) //  ho aggiunto && || map[nesw_cell_index[i]] == start_value
    {
      //std::cout << "Neighbors: " << "(" << nesw_cell_index[i] << ", " << pos << ")" << std::endl;
      neighbors.push_back(std::make_pair( nesw_cell_index[i], pos ));  
    }

  return neighbors.size() > 0;

}

/* 
   Clear map by
   filling it with empty values except start, goal and obstacles

   Params:
      std::vector<int> & map: map to be cleared
*/
void clear_map(std::vector<int> & map)
{
  std::cout << "Clearing map... " << std::endl;
  
  for (int i = 0; i < map.size(); i++)
    if (map[i] != goal_value && map[i] != start_value && map[i] != obstacle_value)
      map[i] = empty_value;
}

/* 
    Wavefront algorithm
    fill map with numbers
    return steps_to_goal (min is 1) if path is found, 0 otherwise

    Params:
      std::vector<int> & map: map to be filled
*/
int fillMap(std::vector<int> & map)
{
  std::cout << "Running wavefront alg to fill map...";

  // La mappa deve contenere solo lo start, il goal e gli ostacoli
  if (start_pos == -1)
    if (!find_start(map, start_pos))
      return 0;

  if (goal_pos == -1)
    if (!find_goal(map, goal_pos))
      return 0;

  // È importante assicurarsi che anche i valori di start e goal siano impostati correttamente
  world_map_vec[start_pos] = start_value;
  world_map_vec[goal_pos] = goal_value;

  clear_map(map);

  // found_path può essere >= 0
  // found_path = 0 cioè false se il path non è stato trovato
  // found_path = steps_to_goal se il path è stato trovato
  // è l'output dell'algoritmo
  int found_path = 0;

  // L'idea:
  // fringe cioè il confine è il fronte d'onda da analizzare
  // sia P il punto di partenza
  // siano A B C D i suoi vicini nell'ordine in cui sono inseriti nel fringe
  // A è tolto dopo che tutti i suoi vicini (ad esempio E, F, G, H) sono stati aggiunti al fringe per essere analizzati
  // poi si passa a B...
  // gli elementi sono quindi analizzati nell'ordine P A B C D E F G H...
  
  // fringe contiene delle coppie, indice della cella + indice della cella padre
  // goal_pos è la prima posizione analizzata dall'algoritmo quindi l'inserisco nel fringe
  std::list< std::pair <int,int> > fringe;
  fringe.push_back(std::make_pair(goal_pos, -1)); // il goal non ha un padre quindi -1 perché è un indice non valido
  
  // l'algoritmo si ferma quando fringe è vuoto
  // oppure quando al primo path trovato, che per la logica dell'algoritmo (breadth-first search) è il migliore
  while (!fringe.empty())
  {

    // Numero di vicini della cella corrente
  	int number_of_neighborns = 0;

  	// fringe_neighbors conterrà i vicini della cella corrente
    std::list< std::pair <int,int> > fringe_neighbors;
    
    // Itera su fringe (all'inizio contiene solo il goal)
    // esci subito se trovi lo start perché è inefficiente continuare a riempire
    std::list< std::pair <int,int> >::const_iterator iterator;
    for (iterator = fringe.begin(); iterator != fringe.end(); ++iterator)
    {
      // Variabili utili per chiarezza
      // indice della cella corrente
      // l'indice del padre della cella corrente
      // il valore della cella corrente
      int cell_index = iterator->first;
      int cell_parent_index = iterator->second;
      int cell_value = map[cell_index];

      // Il valore di un elemento in fringe può essere 1 (goal), 0 (empty) o -1 (start)
      if (cell_value == goal_value)
      {
      	// Il goal è l'elemento di partenza, aggiungo i suoi vicini al fringe
        get_neighbors(cell_index, map, fringe_neighbors, true);
      }
      else if (cell_value == empty_value)
      {
      	// La cella ha valore 0 e devi riempirla con il valore del padre + 1
      	// e aggiungi i suoi vicini al fringe
        map[cell_index] = map[cell_parent_index] + 1;
        get_neighbors(cell_index, map, fringe_neighbors, true);
      }
      else if (cell_value == start_value)
      { 
      	// Hai trovato il path, basta che aggiorni found_path
        found_path = map[cell_parent_index];
        break;
      }
      //else
      //{
      	// Utile per debuggare get_neighbors
      	// non è sempre un errore logico!
      	//std::cout << "ERRORE LOGICO map[iterator->first] " << map[iterator->first] <<  std::endl;
      //}
    
      number_of_neighborns++;

    }

    // Le seguenti modifiche sono fatte dopo il ciclo per non invalidare l'iteratore
    // Aggiungi al fringe tutti i vicini che hai trovato
    fringe.splice(fringe.end(), fringe_neighbors);
    // Togli gli elementi che hai appena considerato nel for
    for (int i = 0; i < number_of_neighborns; i++)
      fringe.pop_front();

    // Perchè la prima soluzione trovata è la migliore
    if (found_path > 0)
  	  break;
      
  }

  std::cout << "steps_to_goal " << found_path << ", done!" << std::endl;

  return found_path;
}

/* 
   Find shortest path

   Params:
     std::vector<int> & path: path represented as sequence of int cardinal directions (north 0 east 1 south 2 west 3)
     std::vector<int> & pos_path: pos_path represented as sequence of indexes of vector representing map
     std::vector<int> & map: map already filled with numbers by fillMap
     int steps_to_goal: number of steps to goal
*/
void find_shortest_path(std::vector<int> & map, std::vector<int> & path, std::vector<int> & pos_path)
{
  std::cout << "Finding shortest path...";

  path.clear();
  pos_path.clear();

  pos_path.push_back(start_pos);

  int i = 0;
  int pos = start_pos;

  while (map[pos] != goal_value)
  {
    
    // Dammi i vicini di pos
    std::list< std::pair <int,int> > neighbors;
    get_neighbors(pos, map, neighbors);

    // Voglio trovare il vicino con il minimo valore
    // 0 è un valore non valido perché confronto solo valori > 0
    int min = 0;

    // Itero sui vicini e trovo quello con il minimo valore
    std::list< std::pair <int,int> >::iterator it;
    for (it = neighbors.begin(); it != neighbors.end(); ++it)
    {
      
      int neighbor_pos = it->first;
      int neighbor_val = map[neighbor_pos];

      if (neighbor_val > empty_value && (min == 0 || neighbor_val < min))
      {
        min = neighbor_val;
        pos = neighbor_pos;
      }

    }
    
    if (pos < pos_path[i] - 1) // north
      path.push_back(north_dir); 
    if (pos == pos_path[i] + 1) // east
      path.push_back(east_dir);
    if (pos > pos_path[i] + 1) // south
      path.push_back(south_dir);
    if (pos == pos_path[i] - 1) // west
      path.push_back(west_dir);

    pos_path.push_back(pos);

    i++;
  
  }

  std::cout << "done!" << std::endl;
}

/*
	Try movement

    Params:
      ros::ServiceClient & client: robot service move_to_cell
      nxt_lab2::MoveToCell & cell_srv: move_to_cell message request
      int cardinal_direction: movement cardinal direction
*/
int try_movement(ros::ServiceClient & client, nxt_lab2::MoveToCell & cell_srv, int cardinal_direction)
{

  cell_srv.request.nesw = cardinal_direction;

  if (client.call(cell_srv))
  {
  
    ROS_INFO("Result of the robot movement (1 = reached, 0 = unreached): %d", cell_srv.response.reached);
    
    if (!cell_srv.response.reached)
    {
      ROS_ERROR("Found unexpected obstacle, cannot continue");
      return false;
    }
  
  }
  else
  {
  
    ROS_ERROR("Failed to call service move_to_cell");
    return false;
  
  }

  return true;

}

/********************************* DYNAMIC MAP FUNCTIONS ********************************/

/* 
   Update map with added or removed obstacles

   Params:
     std::vector<int> & map: map to be updated
     std::vector<int> & neighbors: vector of binary values (0 if no obstacle detected, 1 if obstacle detected)
                                   of length 4, one value for each cell in clockwise order
     int nxt_pos: robot position
*/
bool update_map(std::vector<int> & map, std::vector<int> & neighbors, int nxt_pos)
{
  printVec(neighbors);

  bool added_obstacle = false;
  bool removed_obstacle = false;

  int row_index = get_row_index(nxt_pos);

  int nesw_cell_index[4] = {
    nxt_pos - cols , 
    nxt_pos + 1, 
    nxt_pos + cols, 
    nxt_pos - 1
  };

  bool valid_cell[4] = {
    nxt_pos - cols >= 0,
    nxt_pos + 1 < (row_index + 1) * cols,
    nxt_pos + cols < rows * cols,
    nxt_pos - 1 > row_index * cols - 1
  };

  for(int i = 0; i < 4; i++)
  {
    if(valid_cell[i])
    { 
      if(neighbors[i])
      {
        map[nesw_cell_index[i]] = obstacle_value;
        added_obstacle = true;
      }
      else if(map[nesw_cell_index[i]] == obstacle_value)
      {
      	map[nesw_cell_index[i]] = empty_value;
        removed_obstacle = true;
      }
    }
  }

  return removed_obstacle || added_obstacle;
}

/* 
   Erase map
   fill map with empty values except start and goal

   Params:
      std::vector<int> & map: map to be erased
*/
void erase_map(std::vector<int> & map)
{

  std::cout << "Erasing map..." << std::endl;

  for (int i = 0; i < map.size(); i++)
  {
    if (map[i] != start_value && map[i] != goal_value)
      map[i] = empty_value;
  }

}

/****************************** ROBOT EMULATION *********************************/

/* 
   Emulate robot service moveToCellSrv
   and navigation function try_movement
*/
bool emulate_movement()
{

  bool reached = false;

  sleep(5);

  // rand() is automatically seeded with the a value of 1 if you do not call srand()
  // -> always the same sequence for each run

  srand ( time(NULL) );
  int rand_res = rand() % 15;
  std::cout << "rand_res " << rand_res;
  if (rand_res < 13) // 1/15 true 2/15 false // se mappa statica quando sbaglia termina!
    reached = true;
  else
    ROS_ERROR("Found unexpected obstacle, cannot continue");
  
  return reached;

}

/* 
   Emulate robot service getNeighborsSrv
*/
std::vector<int> emulate_head()
{
  std::vector<int> v;

  srand ( time(NULL) );
  for (int i = 0; i < 4; i++)
  {
    v.push_back(rand() % 2);
    sleep(1);
  }

  std::cout << "Printing emulated obstacles..." << std::endl;
  printVec(v);

  return v;

}

/******************************* MAIN FUNCTION *********************************/

int main(int argc, char **argv) 
{
  
  ros::init(argc, argv, "navigation");

  std::cout << "Hello by navigation" << std::endl;

  ros::NodeHandle n;

  // Read input params from yaml (simplest way)

  // Get rows, cols and boolean dynamic_map

  if(n.getParam("/navigation/rows", rows))
      std::cout << "/navigation/rows " << rows <<  std::endl;
  if(n.getParam("/navigation/cols", cols))
      std::cout << "/navigation/cols " << cols <<  std::endl;
  bool dynamic_map = false;
  if(n.getParam("/navigation/dynamic_map", dynamic_map))
      std::cout << "/navigation/dynamic_map " << dynamic_map <<  std::endl;

  // Get list world_map

  world_map_vec.resize(rows * cols);

  XmlRpc::XmlRpcValue world_map;  
  n.getParam("/navigation/world_map", world_map);

  ROS_ASSERT(world_map.getType() == XmlRpc::XmlRpcValue::TypeArray && world_map.size() == rows * cols);
  for (int32_t i = 0; i < world_map.size(); ++i) 
  {
    ROS_ASSERT(world_map[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
    world_map_vec[i] = static_cast<int>(world_map[i]); // conversion
  }

  // Print map before filling

  std::cout << "Map before filling..." << std::endl;
  printMap(world_map_vec);

  // Wait for services  

  ros::service::waitForService("move_to_cell");
  ros::ServiceClient client = n.serviceClient<nxt_lab2::MoveToCell>("move_to_cell"); // robot service
  nxt_lab2::MoveToCell cell_srv;

  //if (dynamic_map)
  //{
    ros::service::waitForService("get_neighbors");
    ros::ServiceClient neighbors_client = n.serviceClient<nxt_lab2::Neighbors>("get_neighbors"); // robot service
    nxt_lab2::Neighbors neighbors_srv;
  //}

  // Find start and goal

  if (!find_goal(world_map_vec, goal_pos))
  {
  	std::cerr << "Cannot find goal" << std::endl;
    return 0;
  }

  if (!find_start(world_map_vec, start_pos))
  {
  	std::cerr << "Cannot find start" << std::endl;
    return 0;
  }

  // Update nxt position because sent by send_updated_map

  nxt_pos = start_pos;

  // Advertise service send_updated_map

  ros::ServiceServer service = n.advertiseService("send_updated_map", send_updated_map);
  ros::spinOnce();

  /***************************** STATIC OR DYNAMIC ********************************/

  if (!dynamic_map) // map is static
  {

  	std::cout << "Map is static" << std::endl;

    // Fill map

    int steps_to_goal = fillMap(world_map_vec);
    std::cout << "Map after filling..." << std::endl;
    printMap(world_map_vec);

    // Tell path to robot
    if (steps_to_goal > 0)
    {

      // Find shortest path from start to goal
      std::vector<int> path;
      std::vector<int> pos_path;
      find_shortest_path(world_map_vec, path, pos_path);
  
      // Print path for debugging
      printVec(path);

      for (int j = 0; j < path.size(); j++)
      {
        // Try next planned movement
        if (try_movement(client, cell_srv, path[j]))//emulate_movement()) // EMULATION
        {
          nxt_pos = pos_path[j + 1];
          ros::spinOnce();
        }
        else
        {
          // Retry
          //j--;
          //ros::spinOnce();
          return 1;
        }
      }

    }
    else  
      std::cout << "unreachable goal!" << std::endl;
    
  }
  else // map is dynamic
  {

    std::cout << "Map is dynamic" << std::endl;

    while (ros::ok() && nxt_pos != goal_pos) // continuo a cercare fino a quando non raggiungo il goal
	{

	  // cancello il vecchio start
	  // e setto la cella della nuova start_pos
	  world_map_vec[start_pos] = empty_value;
	  start_pos = nxt_pos;
	  world_map_vec[start_pos] = start_value;

      bool any_modifications;
      if (neighbors_client.call(neighbors_srv))
      {
	    std::vector<int> detected_obstacles = neighbors_srv.response.neighbors; // emulate_head() // EMULATION
        std::cout << "DETECTED OBSTACLES" << std::endl;
	    printVec(detected_obstacles);
	    any_modifications = update_map(world_map_vec, detected_obstacles, nxt_pos);
      }
      else
      {
        ROS_ERROR("Failed to call service neighbors_client");
        return 1;
      }

	  if (any_modifications)
	  {
	    std::cout << "Modifications detected..." << std::endl;
	    ros::spinOnce();
	  }

	  int steps_to_goal = fillMap(world_map_vec);
	  std::cout << "Map after filling..." << std::endl;
	  printMap(world_map_vec);

	  if (steps_to_goal > 0)
	  {

	    std::vector<int> path;
	    std::vector<int> pos_path;
	    find_shortest_path(world_map_vec, path, pos_path);

	    // print path for debugging
	    printVec(path);
	    
	    if (try_movement(client, cell_srv, path[0]))//emulate_movement()) // EMULATION
	    {
	      // aggiorno la posizione dell'nxt
	      nxt_pos = pos_path[1];
	      ros::spinOnce();
	    }
	    
	  }
	  else
	  {
	    erase_map(world_map_vec);
	    ros::spinOnce();
	  }
	  
	}

  }

  ros::spin(); // still waiting for send_updated_map requests

  return 0;

}

/**************************** END MAIN FUNCTION *********************************/